package edu.wpi.first.wpilibj.swarm;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Jaguar;

public class MechOutput {
	private LoggerServer logger;
	private DriverStation    ds;
	
	private Jaguar flmotor;
	private Jaguar frmotor;
	private Jaguar rlmotor;
	private Jaguar rrmotor;
	
	private double flout;
	private double frout;
	private double rlout;
	private double rrout;
	private double norm;
	private double maxout;
	
	MechOutput () {
		flmotor = new Jaguar(3);
		frmotor = new Jaguar(5);
		rlmotor = new Jaguar(2);
		rrmotor = new Jaguar(4);
		
		//logger  = new LoggerServer("rot, x, y");
		
		flout   = 0.0;
		frout   = 0.0;
		rlout   = 0.0;
		rrout   = 0.0;
		norm    = 0.0;
		
		ds      = DriverStation.getInstance();
	}
	
	void log(double rot, double x, double y) {
		logger.logString(rot + ", " +
		                 x + ", " +
				         y + "\n");
	}
	
	private void setMaxOut() {
		maxout = ds.getBatteryVoltage()-7.0;
		if (maxout > 1.0) maxout = 1.0;
		if (maxout < 0.0) maxout = 0.0;
	}

	void output(Vector mech_out, double mech_rot_out) {
		//log(mech_rot_out, mech_out.getX(), mech_out.getY());
		
		flout =  mech_out.getX() + mech_out.getY() + mech_rot_out;
		frout = -mech_out.getX() + mech_out.getY() - mech_rot_out;
		rlout = -mech_out.getX() + mech_out.getY() + mech_rot_out;
		rrout =  mech_out.getX() + mech_out.getY() - mech_rot_out;
		
		norm  = Math.max(Math.max(Math.abs(flout), Math.abs(frout)),
		                 Math.max(Math.abs(rlout), Math.abs(rrout)));
		
		setMaxOut();
		
		if (norm > maxout) {
			flout = flout / norm * maxout;
			frout = frout / norm * maxout;
			rlout = rlout / norm * maxout;
			rrout = rrout / norm * maxout;
		}
		
		flmotor.set( flout);
		frmotor.set(-frout);
		rlmotor.set( rlout);
		rrmotor.set(-rrout);
	}
}